// Length of a vector
template <typename T>
T Length( const sf::Vector2<T>& _v ) {
    return sqrt( _v.x * _v.x + _v.y * _v.y );
}

template <typename T>
T SquaredLength( const sf::Vector2<T>& _v ) {
    return _v.x * _v.x + _v.y * _v.y;
}

template <typename T>
T Length( const sf::Vector3<T>& _v ) {
    return sqrt( _v.x * _v.x + _v.y * _v.y + _v.z * _v.z );
}

template <typename T>
T SquaredLength( const sf::Vector3<T>& _v ) {
    return _v.x * _v.x + _v.y * _v.y + _v.z * _v.z;
}

// Zero Vector
template <typename T>
void Zero( sf::Vector2<T>& _v ) {
    _v.x = 0;
    _v.y = 0;
}

template <typename T>
void Zero( sf::Vector3<T>& _v ) {
    _v.x = 0;
    _v.y = 0;
    _v.z = 0;
}

// Is Zero ?
template <typename T>
bool IsZero( const sf::Vector2<T>& _v ) {
    return SquaredLength( _v ) <= ZERO_EPSILON;
}

template <typename T>
bool IsZero( const sf::Vector3<T>& _v ) {
    return SquaredLength( _v ) <= ZERO_EPSILON;
}

// Normalize a vector
template <typename T>
void Normalize( sf::Vector2<T>& _v, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        _v.x *= fInvLgth;
        _v.y *= fInvLgth;
    }
}

template <typename T>
sf::Vector2<T> NormalizedCopy( const sf::Vector2<T>& _v, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        return sf::Vector2<T>( _v.x * fInvLgth, _v.y * fInvLgth );
    } else {
        return _v;
    }
}

template <typename T>
void NormalizedCopy( const sf::Vector2<T>& _v, sf::Vector2<T>& _vOut, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        _vOut.x = _v.x * fInvLgth;
        _vOut.y = _v.y * fInvLgth;
    } else {
        _vOut = _v;
    }
}

template <typename T>
void Normalize( sf::Vector3<T>& _v, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y + _v.z * _v.z );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        _v.x *= fInvLgth;
        _v.y *= fInvLgth;
        _v.z *= fInvLgth;
    }
}

template <typename T>
void NormalizedCopy( const sf::Vector3<T>& _v, sf::Vector3<T>& _vOut, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y + _v.z * _v.z );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        _vOut.x = _v.x * fInvLgth;
        _vOut.y = _v.y * fInvLgth;
        _vOut.z = _v.z * fInvLgth;
    } else {
        _vOut = _v;
    }
}

template <typename T>
sf::Vector3<T> NormalizedCopy( const sf::Vector3<T>& _v, float _fNewLength ) {
    float fLength = sqrt( _v.x * _v.x + _v.y * _v.y + _v.z * _v.z );
    if ( ZERO_EPSILON < fLength ) {
        float fInvLgth = _fNewLength / fLength;
        return sf::Vector3<T>( _v.x * fInvLgth, _v.y * fInvLgth, _v.z * fInvLgth);
    } else {
        return _v;
    }
}

// Truncate a vector
template <typename T>
void Truncate( sf::Vector2<T>& _v, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        Normalize( _v, _fMaxLength );
    }
}

template <typename T>
void TruncateCopy( const sf::Vector2<T>& _v, const sf::Vector2<T>& _vOut, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        Normalize( _vOut, _fMaxLength );
    } else {
        _vOut = _v;
    }
}

template <typename T>
sf::Vector2<T> TruncateCopy( sf::Vector2<T>& _v, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        return NormalizedCopy( _v, _fMaxLength );
    }

    return _v;
}

template <typename T>
void Truncate( sf::Vector3<T>& _v, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        Normalize( _v, _fMaxLength );
    }
}

template <typename T>
void TruncateCopy( const sf::Vector3<T>& _v, const sf::Vector3<T>& _vOut, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        Normalize( _vOut, _fMaxLength );
    } else {
        _vOut = _v;
    }
}

template <typename T>
sf::Vector3<T> TruncateCopy( sf::Vector3<T>& _v, float _fMaxLength ) {
    if ( SquaredLength( _v ) > _fMaxLength * _fMaxLength ) {
        return NormalizedCopy( _v, _fMaxLength );
    }

    return _v;
}

// Dot Product
template <typename T>
T Dot( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 ) {
    return _v1.x * _v2.x + _v1.y * _v2.y;
}

template <typename T>
T Dot( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2 ) {
    return _v1.x * _v2.x + _v1.y * _v2.y + _v1.z * _v2.z;
}

// Cross Product
template <typename T>
void Perp( sf::Vector2<T>& _v ) {
    _v.x = -_v.y;
    _vt.y = _v.x;
}

template <typename T>
sf::Vector2<T> PerpCopy( const sf::Vector2<T>& _v ) {
    return sf::Vector2<T>( -_v.y, _v.x );
}

template <typename T>
void PerpCopy( const sf::Vector2<T>& _v, sf::Vector2<T>& _vOut ) {
    _vOut.x = -_v.y;
    _vOut.y = _v.x;
}

template <typename T>
float Cross( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 ) {
    return _v1.x * _v2.y - _v1.y * _v2.x;
}

template <typename T>
void Cross( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2, sf::Vector3<T>& _vOut ) {
    _vOut.x = _v1.y * _v2.z - _v1.z * _v2.y;
    _vOut.y = _v1.z * _v2.x - _v1.x * _v2.z;
    _vOut.z = _v1.x * _v2.y - _v1.y * _v2.x;
}

template <typename T>
sf::Vector3<T> Cross( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2 ) {
    return sf::Vector3<T>( 
        _v1.y * _v2.z - _v1.z * _v2.y,
        _v1.z * _v2.x - _v1.x * _v2.z,
        _v1.x * _v2.y - _v1.y * _v2.x
        );
}

// Return positive if _v2 is clockwise to _v1, negative if anticlockwise
// Y is pointing down and X pointing right
template <typename T>
int Sign( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 ) {
    if ( Cross( _v1, _v2 ) > 0.0f ) {
        return 1;
    } else {
        return -1;
    }
}